/**
 * \file
 * \date 17.09.2013
 * \author alex
 */
#include <bullet_rviz_test/heightfield_visualization.h>
#include <algorithm>
#include <math.h>
#include <bullet/LinearMath/btMotionState.h>
#include <ros/ros.h>
#include <visualization_msgs/Marker.h>
#include <bullet_rviz_test/heightfield.h>

namespace bullet_rviz_test
{

HeightfieldVisualization::HeightfieldVisualization(
    const HeightfieldPtr & heightfield, uint32_t color) :
    heightfield_(heightfield), color_(color)
{
  heightfield_->registerMotionCallback(
      PhysicalObject::MotionCallback(
          boost::bind(&HeightfieldVisualization::visualize, this)));
}

HeightfieldVisualization::~HeightfieldVisualization()
{
}

void HeightfieldVisualization::visualize()
{
  btTransform world_transform;
  heightfield_->getMotionState()->getWorldTransform(world_transform);
  btVector3 position = world_transform.getOrigin();

  visualization_msgs::MarkerPtr marker = createMarker(world_transform);

  marker->type = visualization_msgs::Marker::TRIANGLE_LIST;

  marker->scale.x = 1;
  marker->scale.y = 1;
  marker->scale.z = 1;

  marker->color = colorFromRgba(color_);

  const btVector3 extents = heightfield_->getExtents();
  size_t x_count = heightfield_->getSizeX() - 1;
  size_t y_count = heightfield_->getSizeY() - 1;
  for (size_t y_index = 0; y_index < y_count; ++y_index)
  {
    for (size_t x_index = 0; x_index < x_count; ++x_index)
    {
      btScalar x0 = position.x()
          + extents.x() * (-0.5f + static_cast<float>(x_index) / x_count);
      btScalar x1 = position.x()
          + extents.x() * (-0.5f + static_cast<float>(x_index + 1) / x_count);
      btScalar y0 = position.y()
          + extents.y() * (-0.5f + static_cast<float>(y_index) / y_count);
      btScalar y1 = position.y()
          + extents.y() * (-0.5f + static_cast<float>(y_index + 1) / y_count);

      float z00 = heightfield_->getData(x_index, y_index);
      float z01 = heightfield_->getData(x_index + 1, y_index);
      float z10 = heightfield_->getData(x_index, y_index + 1);
      float z11 = heightfield_->getData(x_index + 1, y_index + 1);

      geometry_msgs::Point p00, p01, p10, p11;
      fillPoint(p00, x0, y0, position.z() + z00);
      fillPoint(p01, x1, y0, position.z() + z01);
      fillPoint(p10, x0, y1, position.z() + z10);
      fillPoint(p11, x1, y1, position.z() + z11);

      if (isOk(z00) && isOk(z10) && isOk(z01))
      {
        // First triangle:
        marker->points.push_back(p00);
        marker->points.push_back(p10);
        marker->points.push_back(p01);
        marker->colors.push_back(colorFromHeight((p00.z + p10.z + p01.z) / 3));
        marker->colors.push_back(colorFromHeight((p00.z + p10.z + p01.z) / 3));
        marker->colors.push_back(colorFromHeight((p00.z + p10.z + p01.z) / 3));
      }

      if (isOk(z01) && isOk(z10) && isOk(z11))
      {
        // Second triangle:
        marker->points.push_back(p01);
        marker->points.push_back(p10);
        marker->points.push_back(p11);
        marker->colors.push_back(colorFromHeight((p01.z + p10.z + p11.z) / 3));
        marker->colors.push_back(colorFromHeight((p01.z + p10.z + p11.z) / 3));
        marker->colors.push_back(colorFromHeight((p01.z + p10.z + p11.z) / 3));
      }
    }
  }

  publishMarker(marker);
}

void HeightfieldVisualization::fillPoint(geometry_msgs::Point & point,
    geometry_msgs::Point::_x_type x, geometry_msgs::Point::_y_type y,
    geometry_msgs::Point::_z_type z)
{
  point.x = x;
  point.y = y;
  point.z = z;
}

bool HeightfieldVisualization::isOk(geometry_msgs::Point::_z_type z)
{
  return (!isnan(z) && z > (-heightfield_->getExtents().z()));
}

std_msgs::ColorRGBA HeightfieldVisualization::colorFromHeight(float height)
{
  std_msgs::ColorRGBA color;
  float factor = std::max(0.0f,
      std::min(0.5f * (1 + height / heightfield_->getExtents().z()), 1.0f));
  color.r = factor;
  color.g = 0;
  color.b = 1 - factor;
  color.a = 1;
  return color;
}

}
